/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, PAL Robotics, S.L.
 *  All rights reserved.
 *

 *********************************************************************/

#include <chassis_controller/odometry_diff.h>
#include <boost/bind.hpp>


namespace chassis_controller
{
  //namespace bacc = boost::accumulators;
  DiffOdometry::DiffOdometry(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node):Odometry(node)
  {
    timestamp_ = node_->now();
  }

  void DiffOdometry::init(const rclcpp::Time &time, std::vector<Spivot_value> &pivot_value)
  {
      // Reset timestamp:
      timestamp_ = time;
      int pivot_num = pivot_value.size();
  }

  void DiffOdometry::update(std::vector<Spivot_value> &pivot_value, 
    std::vector<Spivot_param> &pivot_param, 
    const rclcpp::Time &time, 
    bool standstill,
    double odom_factor)
  {
    int pivot_num = pivot_value.size();

    const double dt = (time - timestamp_).seconds();
    if (dt < 0.0001) // Interval too small to integrate with
      return; 

    timestamp_ = time;

    //calculate velocity
    pivot_value[0].velocity = (pivot_value[0].joint_value[0].velocity + pivot_value[0].joint_value[1].velocity)/2.0;
    velocity_.y = 0.0;
   
    if (abs(pivot_value[0].velocity) < 0.5) {
      velocity_.x = pivot_value[0].velocity;
      velocity_.a = (pivot_value[0].joint_value[1].velocity - 
        pivot_value[0].joint_value[0].velocity) / 2.0 / pivot_param[0].radius_ringmount;     
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[odom_diff] linear_velocity:" << velocity_.x);
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[odom_diff] angular_velocity:" << velocity_.a);
    } else {
      double liner_velocity = pivot_value[0].velocity;
      //linear_acc_(liner_velocity);
      //velocity_.x = bacc::rolling_mean(linear_acc_);
      velocity_.x = meanFilter(liner_velocity, linear_vec_, 5);
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[odom_diff] rolling linear_velocity:" << velocity_.x);
      if (pivot_param[0].radius_ringmount >1e-6) {
        double angular_velocity = (pivot_value[0].joint_value[1].velocity - 
          pivot_value[0].joint_value[0].velocity) / 2.0 / pivot_param[0].radius_ringmount;
        //angular_acc_(angular_velocity);
        //velocity_.a = bacc::rolling_mean(angular_acc_);
        velocity_.a = meanFilter(angular_velocity, angular_vec_, 5);
        DEBUG_OUT_THROTTLE(get_clock(),
          500, "[odom_diff] rolling angular_velocity:"<<velocity_.a);
      } else {
        DEBUG_OUT("radius_ringmount equal zero");
        return;
      }              
    }
  
    double left_pos ,right_pos , left_wheel_radius , right_wheel_radius;
    left_pos = pivot_value[0].joint_value[0].position;
    right_pos = pivot_value[0].joint_value[1].position;
    left_wheel_radius = pivot_param[0].joint_param[0].wheel_diameter / 2.0;
    right_wheel_radius = pivot_param[0].joint_param[1].wheel_diameter / 2.0;
    
    DEBUG_OUT_THROTTLE(get_clock(),
        1000, "[odom_diff] left_wheel_radius:"<<left_wheel_radius);

    double left_wheel_cur_pos = left_pos;   //* left_wheel_radius;
    double right_wheel_cur_pos = right_pos; //* right_wheel_radius;

    /// Estimate velocity of wheels using old and current position:
    double delta_pos_left = (left_wheel_cur_pos - previous_pos_left_) * odom_factor;
    double delta_pos_right = (right_wheel_cur_pos - previous_pos_right_) * odom_factor;

    previous_pos_left_ = left_wheel_cur_pos;
    previous_pos_right_ = right_wheel_cur_pos;

    //RCLCPP_INFO_STREAM_THROTTLE(0.5, "[kinematics_diff] delta_pos :"<<delta_pos_left <<", "<<delta_pos_right);
    if (delta_pos_left > 0.1 || delta_pos_right > 0.1 )
    {
      delta_pos_left = 0.0;
      delta_pos_right = 0.0;
      //RCLCPP_INFO_STREAM_THROTTLE(0.5, "[kinematics_diff] postion increment oversize");
    }

    //step2: calculate delta theta of vehicle and angular velocity
    double delta_offset_x , delta_offset_y , delta_theta , omiga;
    delta_theta = 0.0;
     
    if (pivot_param[0].radius_ringmount > 1e-6){
      //delta_theta = (delta_pos_right - delta_pos_left)/pivot_param[0].radius_ringmount*2.0;
      delta_theta = velocity_.a * dt;
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[kinematics_diff] delta_theta :" <<delta_theta);
    }
    double delta_pos_x = (delta_pos_right + delta_pos_left)/2.0;
    DEBUG_OUT_THROTTLE(get_clock(),
      500, "[kinematics_diff] delta_pos_x :" <<delta_pos_x);

    //accumulate position of vehicle
    Sxya delta_pos;    //accumulate position of vehicle in map coordinate
    delta_pos.x =delta_pos_x;
    delta_pos.y =0.0;
    delta_pos.a =delta_theta;

    if(standstill){
      delta_pos.x = 0.0;
      delta_pos.y = 0.0;
      delta_pos.a = 0.0;
    }

    // pos_vehicle_.x += delta_pos.x;
    // pos_vehicle_.y = 0.0;
    // pos_vehicle_.a += delta_pos.a;
    // pos_vehicle_.a = normAngle2(pos_vehicle_.a);

    //accumulate position of vehicle in map coordinate
    integrateExact(delta_pos.x, delta_theta);
    
    // Sxya delta_pos_map;
    // delta_pos_map = frameTrans(delta_pos, position_.a);
    // position_.x += delta_pos_map.x;
    // position_.y += delta_pos_map.y;
    // position_.a += delta_pos_map.a;
    // position_.a = normAngle2(position_.a);

    DEBUG_OUT_THROTTLE(get_clock(),
      500, "position  :" <<position_.x <<"," <<position_.y <<"," <<position_.a );

  }

  Sxya DiffOdometry::frameTrans(Sxya original , double angle)
  { 
    Sxya target;
    target.x = original.x * cos(angle) - original.y * sin(angle);
    target.y = original.x * sin(angle) + original.y * cos(angle);
    target.a = original.a;
    return target;
  }

  void DiffOdometry::integrateRungeKutta2(double delta_x, double delta_a)
  {
    const double direction = position_.a + delta_a * 0.5;

    /// Runge-Kutta 2nd order integration:
    position_.x += delta_x * cos(direction);
    position_.y += delta_x * sin(direction);
    position_.a += delta_a;
  }


  void DiffOdometry::integrateExact(double delta_x, double delta_a)
  {
    if (fabs(velocity_.a) < 1e-6)
      integrateRungeKutta2(delta_x, delta_a);
    else
    {
      /// Exact integration (should solve problems when angular is zero):
      const double heading_old = position_.a;
      const double r = velocity_.x/velocity_.a;
      position_.a += delta_a;
      position_.x +=  r * (sin(position_.a) - sin(heading_old));
      position_.y += -r * (cos(position_.a) - cos(heading_old));
    }
    position_.a = normAngle2(position_.a);
  } 

  double DiffOdometry::meanFilter(double input, std::vector<double> &vector, int size){
    vector.push_back(input);
    double sum = 0.0;
    if (vector.size()>size){
      vector.erase(vector.begin());
    }

    for(auto iter : vector){
      sum += iter;
    }

    if (!vector.empty()){
      //sum = std::accumulate(std::begin(vector), std::end(vector), 0);
      double mean = static_cast<double>(sum)/vector.size();
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[meanFilter]size,sum,mean:" <<vector.size()<<","<<sum<<","<<mean);
      return mean;
    } else {
      return 0.0;
    }
  }

  // void DiffOdometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
  // {
  //   velocity_rolling_window_size_ = velocity_rolling_window_size;

  //   resetAccumulators();
  // }

  // void DiffOdometry::resetAccumulators()
  // {
  //   linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
  //   angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
  //   // linear_acc_ = RollingMeanAcc(RollingWindow::window_size = 2);
  //   // angular_acc_ = RollingMeanAcc(RollingWindow::window_size = 2);
  // }

}
